#include "CStereoCamera.h"
#include <time.h>
#include "cvxCapture.h"
#include "cvxSf.h"

#if 0
//test chessboard from two videos
int main()
{
	CvCapture *leftCap = cvCaptureFromFile("video\\20111117\\rotate-z90\\960\\bot_z90_960.avi");
	CvCapture *rightCap = cvCaptureFromFile("video\\20111117\\rotate-z90\\960\\top_z90_960.avi");

	assert(leftCap);
	assert(rightCap);

	IplImage *leftImage = cvQueryFrame(leftCap);
	IplImage *rightImage = cvQueryFrame(rightCap);

	vector<Point2f> pts1;
	vector<Point2f> pts2;
	IplImage *leftShow  = cvCreateImage(cvGetSize(leftImage), IPL_DEPTH_8U, 3);
	IplImage *rightShow = cvCreateImage(cvGetSize(leftImage), IPL_DEPTH_8U, 3);
	while(leftImage && rightImage)
	{
		pts1.clear();
		pts2.clear();
		bool isok = CvxSF::GetChessboard(leftImage, cvSize(15, 9), false, pts1);
		if (isok)
		{
			isok = CvxSF::GetChessboard(rightImage, cvSize(15, 9), false, pts2);
		}
		cvCopy(leftImage, leftShow);
		cvCopy(rightImage, rightShow);
		if (isok)
		{
			cv::drawChessboardCorners(Mat(leftShow), cvSize(15, 9), pts1, true);
			cv::drawChessboardCorners(Mat(rightShow), cvSize(15, 9), pts2, true);
		}
		cvShowImage("left", leftShow);
		cvShowImage("right", rightShow);
		cvWaitKey(1);
		leftImage = cvQueryFrame(leftCap);
		rightImage = cvQueryFrame(rightCap);

	}
	cvWaitKey(0);
	return 1;

}
#endif

#if 0
//align a sequence of images
int main()
{

	CCamera botC;
	CCamera topC;

	botC.loadCamera("save_image\\15_9\\bottom\\960\\bottom_960_cam.txt");
	topC.loadCamera("save_image\\15_9\\top\\960\\top_960_cam.txt");

	cv::Mat botMatrix = botC.getIntrinsicMatrix();
	cv::Mat botDistortion = botC.getDistortionCoeffs();
	cv::Mat topMatrix = topC.getIntrinsicMatrix();
	cv::Mat topDistortion = topC.getDistortionCoeffs();

	string botVideoFileName = string("video\\15_9\\bottom_23345300_15_9_960.avi");
	string topVideoFileName = string("video\\15_9\\top_23345209_15_9_960.avi");

	string videoFileName;
	vector<int> Idx;
	vector<vector<Point2f> > botChessboardPos;
	vector<vector<Point2f> > topChessboardPos;
	CvSize chessboardSize;
	CChessBoard board(cvSize(15, 9), 0.025);
	
//	CvxSF::saveStereoChessboard(botVideoFileName, topVideoFileName, string("bot_top_960_ckb_pos.txt"), 24, cvSize(15, 9), true);
	
	CvxSF::loadStereoChessboard(string("video\\15_9\\bot_top_960_ckb_pos.txt"), botVideoFileName, topVideoFileName, 
								Idx, chessboardSize, botChessboardPos, topChessboardPos);

	Mat rmap[2][2];

	CStereoCamera::AlignImageSequence(botMatrix, botDistortion, 
					topMatrix, topDistortion,
					board._pts3d,
					botChessboardPos, 
					topChessboardPos,
					cvSize(960, 540),
					0.5, rmap);

	CvCapture *botCap = cvCaptureFromFile(botVideoFileName.c_str());
	CvCapture *topCap = cvCaptureFromFile(topVideoFileName.c_str());

	int frameNum = 0;
	IplImage *leftImage = cvQueryFrame(botCap);
	IplImage *rightImage = cvQueryFrame(topCap);

	IplImage *leftWarp = cvCloneImage(leftImage);
	IplImage *rightWarp = cvCloneImage(rightImage);
	Mat canvas = Mat(leftWarp->height, leftWarp->width*2, CV_8UC3);
	IplImage canvasImg = canvas;
	string toFileName = string("960_align.avi");

	CvVideoWriter *writer = cvCreateAVIWriter(toFileName.c_str(), CV_FOURCC('M','J','P','G'), 24, cvSize(960*2*0.8, 540*0.8), 1); 
	while(leftImage && rightImage)
	{
		
		cv::remap(Mat(leftImage), Mat(leftWarp), rmap[0][0], rmap[0][1], CV_INTER_LINEAR);
		cv::remap(Mat(rightImage), Mat(rightWarp), rmap[1][0], rmap[1][1], CV_INTER_LINEAR);	
		

		Mat canvasPart = canvas(cv::Rect(0, 0, leftWarp->width, leftWarp->height));
		cv::resize(Mat(leftWarp), canvasPart, canvasPart.size());

		canvasPart = canvas(Rect(leftWarp->width, 0, leftWarp->width, leftWarp->height));
		cv::resize(Mat(rightWarp), canvasPart, canvasPart.size(), 0, 0, CV_INTER_AREA);
		

		for (int i = 0; i<leftWarp->height; i += leftWarp->height/20)
		{
			CvScalar colors[3] = {cvScalar(0, 0, 255), cvScalar(0, 255, 0), cvScalar(255, 0, 0)};
			cv::line(canvas, Point(0, i), Point(leftWarp->width*2, i),
						colors[i/(leftWarp->height/20)%3]);
		}
		cvWriteToAVI(writer, &canvasImg);

		cv::imshow("warp", canvas);
		cvWaitKey(10);

		leftImage = cvQueryFrame(botCap);
		rightImage = cvQueryFrame(topCap);
	}
	cvReleaseVideoWriter(&writer);
	
	cvWaitKey(0);
	return 1;

}

#endif

#if 0
//align one pair images
int main()
{
	
	CCamera botC;
	CCamera topC;

	botC.loadCamera("save_image\\15_9\\bottom\\960\\bottom_960_cam.txt");
	topC.loadCamera("save_image\\15_9\\top\\960\\top_960_cam.txt");

	cv::Mat botMatrix = botC.getIntrinsicMatrix();
	cv::Mat botDistortion = botC.getDistortionCoeffs();
	cv::Mat topMatrix = topC.getIntrinsicMatrix();
	cv::Mat topDistortion = topC.getDistortionCoeffs();


	CvCapture *botCapture = cvCaptureFromFile("video\\15_9\\bottom_23345300_15_9_960.avi");
	CvCapture *topCapture = cvCaptureFromFile("video\\15_9\\top_23345209_15_9_960.avi");

	assert(botCapture);
	assert(topCapture);

	string toFileName = string("video\\15_9\\15_9_align_24.avi");
	CvVideoWriter *writer = cvCreateAVIWriter(toFileName.c_str(), CV_FOURCC('M','J','P','G'), 24, cvSize(960*2*0.8, 540*0.8), 1); 

	while(1)
	{
		IplImage *leftImage = cvQueryFrame(botCapture);
		IplImage *rightImage = cvQueryFrame(topCapture);

		if (!leftImage || !rightImage)
		{
			break;
		}	

		CChessBoard board(cvSize(15, 9), 0.025);

		vector<Point2f> botPts;
		vector<Point2f> topPts;
		int botOk = CvxSF::GetChessboard(leftImage, board.size, false, botPts);
		int topOk = CvxSF::GetChessboard(rightImage, board.size, false, topPts);
		if (botOk && topOk)
		{
			if (botPts[0].y > botPts[15].y)
			{
				std::reverse(botPts.begin(), botPts.end());
			}
			if (topPts[0].y > topPts[15].y)
			{
				std::reverse(topPts.begin(), topPts.end());
			}

			IplImage *leftAlign = cvCreateImage(cvGetSize(leftImage), IPL_DEPTH_8U, 3);
			cvZero(leftAlign);
			IplImage *rightAlign = cvCloneImage(leftAlign);

			//align two images
			double err = CStereoCamera::AlignTwoImages(botMatrix, botDistortion, 
				topMatrix, topDistortion,
				board._pts3d,
				botPts, 
				topPts,
				leftImage, rightImage, leftAlign, rightAlign, 1.0);

			Mat canvas = Mat(leftAlign->height, leftAlign->width*2, CV_8UC3);

			Mat canvasPart = canvas(cv::Rect(0, 0, leftAlign->width, leftAlign->height));
			cv::resize(leftAlign, canvasPart, canvasPart.size());

			canvasPart = canvas(Rect(leftAlign->width, 0, leftAlign->width, leftAlign->height));
			cv::resize(rightAlign, canvasPart, canvasPart.size(), 0, 0, CV_INTER_AREA);
			for (int i = 0; i<leftImage->height; i += leftImage->height/20)
			{
				CvScalar colors[3] = {cvScalar(0, 0, 255), cvScalar(0, 255, 0), cvScalar(255, 0, 0)};
				cv::line(canvas, Point(0, i), Point(leftImage->width*2, i),
					colors[i/(leftImage->height/20)%3]);
			}

			IplImage canvasImg = canvas;

			char buf[255] = {NULL};
			const CvFont font = cvFont(2.0, 1);
			sprintf(buf, "error : %f", err);
			cvPutText(&canvasImg, buf, cvPoint(0, 540-40), &font, cvScalar(0, 0, 255));
			cv::imshow("align", canvas);			

			cvWriteToAVI(writer, &canvasImg);
			
			cvWaitKey(50);

			cvReleaseImage(&leftAlign);
			cvReleaseImage(&rightAlign);
		}
		else
		{
			printf("can not found chessboard\n");
		}
	}
	cvReleaseVideoWriter(&writer);	

	cvWaitKey(0);
	return 1;
}

#endif

#if 0
//record rotate angle and translation distance in a video file
int main()
{
	double tt = clock();	

	CCamera botC;
	CCamera topC;

	botC.loadCamera("save_image\\15_9\\bottom\\960\\bottom_960_cam.txt");
	topC.loadCamera("save_image\\15_9\\top\\960\\top_960_cam.txt");

	cv::Mat botMatrix = botC.getIntrinsicMatrix();
	cv::Mat botDistortion = botC.getDistortionCoeffs();
	cv::Mat topMatrix = topC.getIntrinsicMatrix();
	cv::Mat topDistortion = topC.getDistortionCoeffs();

	CvCapture *botCapture = cvCaptureFromFile("video\\15_9\\bottom_23345300_15_9_960.avi");
	CvCapture *topCapture = cvCaptureFromFile("video\\15_9\\top_23345209_15_9_960.avi");
	
	assert(botCapture);
	assert(topCapture);

	CChessBoard board(cvSize(15, 9), 0.025);

	string toFileName = string("video\\15_9\\15_9_960_tanslation_angle_24.avi");
 	CvVideoWriter *writer = cvCreateAVIWriter(toFileName.c_str(), CV_FOURCC('M','J','P','G'), 24, cvSize(960*2*0.8, 540*0.8), 1); 
 	if (!writer)
 	{
 		printf("can not create %s video writer\n", toFileName.c_str());
 		return false;
 	}

	while(1)
	{
		IplImage *topImage = cvQueryFrame(topCapture);
		IplImage *botImage = cvQueryFrame(botCapture);
		if (!topImage || !botImage)
		{
			break;
		}
		float theta[3] = {0};
		cv::Mat tvec = cv::Mat(3, 1, CV_64F);
		vector<Point2f> botPts;
		vector<Point2f> topPts;
		int botOk = CvxSF::GetChessboard(botImage, board.size, false, botPts);
		int topOk = CvxSF::GetChessboard(topImage, board.size, false, topPts);
		if (botOk && topOk)
		{
			if (botPts[0].y > botPts[15].y)
			{
				std::reverse(botPts.begin(), botPts.end());
			}
			if (topPts[0].y > topPts[15].y)
			{
				std::reverse(topPts.begin(), topPts.end());
			}
			
			CStereoCamera::GetRotateTranslation(botMatrix, botDistortion, topMatrix, topDistortion,
				board._pts3d, botPts, topPts, cvSize(960, 540), theta[0], theta[1], theta[2], tvec);
		}

		//save image
		{
			IplImage *topCopy = cvCloneImage(topImage);
			IplImage *botCopy = cvCloneImage(botImage);
			if (botOk && topOk)
			{
				char topBuf[255] = {NULL};
				char botBuf[255] = {NULL};
				const CvFont font = cvFont(2.0, 1);
				sprintf(topBuf, "angle      : %f %f %f", theta[0], theta[1], theta[2]);
				sprintf(botBuf, "translation: %f %f %f", tvec.at<double>(0, 0), tvec.at<double>(1, 0), tvec.at<double>(2, 0));
				cvPutText(topCopy, topBuf, cvPoint(0, 540-40), &font, cvScalar(0, 0, 255));
				cvPutText(topCopy, botBuf, cvPoint(0, 540-20), &font, cvScalar(0, 0, 255));

				cv::drawChessboardCorners(Mat(topCopy), cvSize(15, 9), topPts, true);
				cv::drawChessboardCorners(Mat(botCopy), cvSize(15, 9), botPts, true);

			}
			IplImage *combineImage = cvCreateImage(cvSize(topCopy->width*2, topCopy->height), IPL_DEPTH_8U, 3);
			cvSetImageROI(combineImage, cvRect(0, 0, topCopy->width, topCopy->height));
			cvCopy(topCopy, combineImage);
			cvResetImageROI(combineImage);

			cvSetImageROI(combineImage, cvRect(topCopy->width, 0, topCopy->width, topCopy->height));
			cvCopy(botCopy, combineImage);
			cvResetImageROI(combineImage);

			cvShowImage("combine", combineImage);
			cvWriteToAVI(writer, combineImage);
			cvWaitKey(30);
			if (theta[0] > 1.0)
			{
				cvWaitKey(0);
			}
			cvReleaseImage(&combineImage);
		}
	}
	cvReleaseVideoWriter(&writer);
	
	cvWaitKey(0);
	return 1;
}
#endif